#include "forkliftdrive.h"

bool ForkliftDrive::CanOpenAndStart() {
    uint32 dwRel;

    dw_DeviceHandle = canDevice->CanDeviceOpen( CAN_DEVICE_TYPE, dw_Index, nullptr );
    qDebug() << "ForkliftDrive dw_DeviceHandle" << dw_DeviceHandle;
    if ( dw_DeviceHandle == 0 ) { return false; }
    // 配置CAN通道参数
    CanInitConfig config;
    config.dwAccCode  = 0;
    config.dwAccMask  = 0xffffffff;
    config.nFilter    = 0;        // 滤波方式(0表示未设置滤波功能,1表示双滤波,2表示单滤波)
    config.bMode      = 0;        // 工作模式(0表示正常模式,1表示只听模式)
    config.nBtrType   = 1;        // 位定时参数模式(1表示SJA1000,0表示LPC21XX)
    config.dwBtr[ 0 ] = dw_Btr0;  // BTR0   0014 -1M 0016-800K 001C-500K 011C-250K 031C-125K 041C-100K 091C-50K 181C-20K 311C-10K BFFF-5K
    config.dwBtr[ 1 ] = dw_Btr1;  // BTR1
    config.dwBtr[ 2 ] = 0;
    config.dwBtr[ 3 ] = 0;
    dwRel             = canDevice->CanChannelStart( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel, &config );
    if ( dwRel != CAN_DEVICE_RESULT_OK ) {
        qDebug() << "fail，dw_Channel:0";
        return false;
    }
    is_open_can = true;
    return true;
}

bool ForkliftDrive::CanStopAndClose() {
    if ( is_open_can ) {
        if ( canDevice->CanChannelStopDeviceClose( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel ) ) {
            is_open_can = false;
            qDebug() << "成功，CAN关闭";
            return true;
        }
    }

    return false;
}

void ForkliftDrive::CanSendData( CanDataFrame* frames, unsigned long i ) {
    if ( i == 0 ) return;
    if ( FRAMES_CNT >= i )
        canDevice->CanChannelSend( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel, frames, i );
    else {
        for ( ; i > FRAMES_CNT; i -= FRAMES_CNT )
            canDevice->CanChannelSend( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel, frames, FRAMES_CNT );
        if ( i > 0 ) canDevice->CanChannelSend( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel, frames + FRAMES_CNT, i );
    }
    for ( uint count = 0; count < i; count++ ) {
        QByteArray recv;
        for ( int k = 0; k < frames[ count ].nDataLen; k++ ) {
            recv.append( static_cast< char >( frames[ count ].arryData[ k ] ) );
        }
        emit sigShowCanData( recv, "Send:" + QString::number( frames[ count ].uID, 16 ) );  //发送打印static_cast<short int>
    }
}
void ForkliftDrive::AccDecUpdate( double& acc, double& dec ) {
    Q_UNUSED( acc )
    Q_UNUSED( dec )
    // todo
}
void ForkliftDrive::GetCanRecivNaviParam( double* walkspeed, double* veerad ) {
    if ( !is_open_can ) { return; }
    *walkspeed = fork_walk_reality_speed;  //叉车直行速度反馈
    *veerad    = fork_veer_reality_rad;    //叉车转角弧度反馈
}
void ForkliftDrive::push_fork_upper() {
    kds_fall       = 0;
    kds_rise       = 1;
    fork_lft_speed = 100;
}
void ForkliftDrive::push_fork_stop() {
    kds_fall       = 0;
    kds_rise       = 0;
    fork_lft_speed = 0;
}
void ForkliftDrive::push_fork_down() {
    kds_rise       = 0;
    kds_fall       = 100;
    fork_lft_speed = 0;
}

void ForkliftDrive::switchAutoModel() {
    qDebug() << "swithcAutoModel 自动模式";
}

void ForkliftDrive::switchHandleModel() {
    qDebug() << "swithcHandleModel 手动模式";
}

void ForkliftDrive::CanDataFrameConfig( CanDataFrame& frame, unsigned int uID, unsigned char arryData[], unsigned char nDataLen ) {
    frame.uID         = uID;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = nDataLen;
    for ( int i = 0; i < nDataLen; i++ ) {
        frame.arryData[ i ] = arryData[ i ];
    }
}
void ForkliftDrive::GetDriveErrInfo() {
    qDebug() << "ForkliftDrive::GetDriveErrInfo";
}
void ForkliftDrive::DealDriveErrInfo( QString str, int err_info ) {}

void ForkliftDrive::sltTimeOut() {
    if ( !is_open_can ) { return; }
    //定期发送
    RegularGetData();
    //定期接收
    CanRecviveData();
}
